#pragma once
#include <rclcpp/rclcpp.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <action_msgs/msg/goal_status.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <yaml-cpp/yaml.h>
#include <std_srvs/srv/trigger.hpp>
class CorrectPose : public rclcpp::Node
{

public:
    CorrectPose() : Node("correct_pose")
    {
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        // 初始化服务服务器
        trigger_service_ = this->create_service<std_srvs::srv::Trigger>(
            "trigger_pub_initial_pose",
            std::bind(&CorrectPose::handleTriggerService, this, std::placeholders::_1, std::placeholders::_2));
        amcl_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/amcl_pose", 10,
            std::bind(&CorrectPose::amclCallBack, this, std::placeholders::_1));
        pub_initial_pose_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/initialpose", 10);
        pub_tf_ = this->create_publisher<tf2_msgs::msg::TFMessage>(
            "/tf", 10);
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            [this](const nav_msgs::msg::Odometry::SharedPtr msg)
            {
                // 处理里程计数据
                if (fabs(msg->twist.twist.linear.x) < 0.01 && fabs(msg->twist.twist.linear.y) < 0.01 && fabs(msg->twist.twist.angular.z) < 0.01)
                {
                    // 里程计数据有效，进行坐标转换
                    correct_pose_flag_ = true;
                }
                else
                {
                    correct_pose_flag_ = false;
                }
            });
        // 订阅动态和静态TF话题
        tf_sub_ = create_subscription<tf2_msgs::msg::TFMessage>(
            "/tf", 1000,
            [this](const tf2_msgs::msg::TFMessage::SharedPtr msg)
            {
                processTF(msg->transforms);
            });

        tf_static_sub_ = create_subscription<tf2_msgs::msg::TFMessage>(
            "/tf_static", 1000,
            [this](const tf2_msgs::msg::TFMessage::SharedPtr msg)
            {
                processTF(msg->transforms);
            });

        // 创建定时器定期查询变换
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(10),
            std::bind(&CorrectPose::Callback, this));
    }

private:
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
    rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_static_sub_;
    std::vector<std::string> tag_frames_;
    std::mutex mutex_;
    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_initial_pose_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr amcl_pose_sub_;
    rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;

    bool correct_pose_flag_ = false;
    bool previous_correct_pose_flag_ = false; // 上一次 correct_pose_flag_ 状态
    rclcpp::Time flag_switch_time_;           // 记录 flag 切换的时间
    geometry_msgs::msg::PoseWithCovarianceStamped amcl_pose_;
    // 服务服务器
    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr trigger_service_;

    // 服务回调函数
    void handleTriggerService(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
                              std::shared_ptr<std_srvs::srv::Trigger::Response> response);
    void processTF(const std::vector<geometry_msgs::msg::TransformStamped> &transforms);
    void Callback();
    void pubInitialPose(YAML::Node yaml_node);
    void amclCallBack(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
    void pubMapOdomTransform();
};